package graphlib;

import dk.itu.KrakMap.KrakEdge;
import dk.itu.KrakMap.KrakNode;
import java.util.*;

/**
 * Stores a graph using an adjacency list.
 * The graph can contain any mix of directed and undirected edges.
 * 
 * @author Peter Tiedemann petert@itu.dk
 */
public class Graph<E extends Edge<N>, N extends Node> {
    // sestoft: Changed edge representation from linked list to array list.
    // Important to create them small: 3 resp. 1 items because the average
    // node degree is very low.

    /**
     * This is the standard adjacency list representation of a graph in
     * edges[i], all edges leaving the node with index i is stored in an
     * array list. This means an undirected edge (a,b) can be found in
     * edges[a] and edges[b], and the Edge object referenced in both is
     * the same
     */
    public ArrayList<ArrayList<E>> edges;
    /**
     * In order to allow traversing the graph against normal edge
     * directions (for backwards search or similar), this adjacency list
     * stores at reverse_edges[i] DIRECTED edges ENTERING node
     * i. Undirected edges have no reverse mapping as they are already
     * stored in both directions.
     *
     * The reverse mapping is stored seperatly to avoid having to
     * iterate over these edges during normal traversal. If one traveses
     * the graph in backwards direction, one has to iterate over both
     * the contents of edges and reverse_edges.  This could be avoided
     * by using 3 adjacency lists, one for undirected edges, ,one for
     * directed edges in the normal direction, and one storing directed
     * edges backwards.
     */
    ArrayList<ArrayList<E>> reverse_edges;
    /**
     * An array of Node objects, representing the nodes of the graph
     */
    public ArrayList<N> nodes;
    /**
     * A counter storing the total number of edges in the graph
     */
    int edgeCount = 0;

    /**
     * Returns the total number of edges in the graph
     * (undirected counted once)
     * @return
     */
    public int getEdgeCount() {
        return edgeCount;
    }

    /**
     * Creates an edge-less graph on the Node objects in the array list.
     */
    public Graph(ArrayList<N> nodes) {
        int numNodes = nodes.size();

        // NB: To match the indexes 1,2,... used in the file
        // we leave an empty slot for node 0

        edges = new ArrayList<ArrayList<E>>(numNodes + 1);
        reverse_edges = new ArrayList<ArrayList<E>>(numNodes + 1);

        for (int i = 0; i <= numNodes + 1; i++) {
            edges.add(new ArrayList<E>(3));
            reverse_edges.add(new ArrayList<E>(1));
        }

        this.nodes = new ArrayList<N>(numNodes + 1);
        this.nodes.add(null);
        for (int i = 1; i <= numNodes; i++) {
            this.nodes.add(nodes.get(i - 1));
        }
    }

    private Graph(ArrayList<N> nodes, ArrayList<ArrayList<E>> edges, ArrayList<ArrayList<E>> reverse_edges, int edge_count) {
        this.nodes = nodes;
        this.edges = edges;
        this.reverse_edges = reverse_edges;
        this.edgeCount = edge_count;
    }

    /**
     * Adds edges contained in a collection to the graph
     * @param c A collection of Edge objects
     */
    public void addEdges(Collection<E> c) {
        Iterator<E> it = c.iterator();
        while (it.hasNext()) {
            E e = it.next();
            this.addEdge(e);
        }
    }

    /**
     * Adds the given edge to the graph
     * @param e edge to add
     */
    public void addEdge(E e) {
        if (e.v1.index >= edges.size() || e.v2.index >= edges.size()) {
            throw new IllegalArgumentException("Attempting to add edge to graph, connecting non-existing nodes " + e.v1.index + " " + e.v2.index);
        }
        edges.get(e.getStartNode().index).add(e);
        if (e.direction != Edge.BOTH) {
            // we only need reverse mapping for undirected edges
            reverse_edges.get(e.getEndNode().index).add(e);
        } else if (e.direction == Edge.BOTH) {
            edges.get(e.getEndNode().index).add(e);
        }
        edgeCount++;
    }

    /**
     * Returns the Node object corresponding to the node index given.
     * @param index
     * @return
     */
    public N getNode(int index) {
        if (nodes.get(index) == null) {
            throw new RuntimeException("No node at " + index);
        }
        return nodes.get(index);
    }

    /**
     * Returns a list over all outgoing edges
     * (includes undirected edges) from the node n
     * Theres a method for getting it both as a list and an iterator
     * @param n
     * @return
     */
    public List<E> outGoingEdgesList(Node n) {
        return edges.get(n.getIndex());
    }

    public Iterator<E> outGoingEdges(Node n) {
        try {
            return edges.get(n.index).iterator();
        } catch (NullPointerException e) {
            //System.out.println("no elements");
        }
        return null;
    }

    /**
     * Returns an iterator over all incoming edges
     * (excludes undirected edges)
     * @param n
     * @return
     */
    public Iterator<E> incomingEdges(Node n) {
        try {
            return reverse_edges.get(n.index).iterator();
        } catch (NullPointerException e) {
            //System.out.println("no elements");
        }
        return null;
    }

    public Iterator<E> allEdges(Node n) {
        
            Iterator it1 = incomingEdges(n);
            Iterator it2 = outGoingEdges(n);
        if(it1 != null && it2 != null){
            return new CombinedIterator(new Iterator[]{it1, it2});
                }
        
        return null;
    }

    /**
     * Find the easiest route as the route with the lowest total weight of
     * edges.
     * @param start The node to start from
     * @param end The node to end at
     * @return A list of edges in order as in how to get from start to end.
     */
     public List<E> easiestRoute(N start, N end){
        return djikstra(start, end);
     }

    /**
     * Find the easiest route, as the route with the lowest total weight of
     * edges, utilizing Heuristic estimates to speed up the process.
     * @param start The node to start from
     * @param end The node to end at
     * @param h The Heuristics to use for the search
     * @return A list of edges in order as in how to get from start to end.
     */
    public List<E> easiestRoute(N start, N end, Heuristics h){
        return aStar(start,end,h);
    }

    //Variables for Djikstra
    private List<E> djikstra(N start, N end) {
        final HashMap<N, E> cameFrom = new HashMap<N, E>();
        final HashMap<N, Double> costTo = new HashMap<N, Double>();
        final HashSet<N> hasVisited = new HashSet<N>();
        final PriorityQueue<N> queue = new PriorityQueue<N>(10000, new Comparator<N>() {
            public int compare(N n1, N n2)
            {
                double v1;
                double v2;
                if (costTo.get(n1) == null) {
                    v1 = Double.POSITIVE_INFINITY;
                } else {
                    v1 = costTo.get(n1);
                }
                if (costTo.get(n2) == null) {
                    v2 = Double.POSITIVE_INFINITY;
                } else {
                    v2 = costTo.get(n2);
                }
                if (v1 > v2) {
                    return 1;
                } else if (v1 < v2) {
                    return -1;
                } else {
                    return 0;
                }
            }
        });
        
        costTo.put(start, 0.0);
        queue.offer(start);

        while (!queue.isEmpty()) {
            //Get the next in the queue
            N n = queue.poll();
            
            //Return the path if we are at the destination
            if (n == end) return getPathToTarget(end, cameFrom);

            hasVisited.add(n); //Add node to list of visited nodes...

            //For each outGoingEdge:
            for (E e : outGoingEdgesList(n)) {
                N n2 = e.getOtherEndNode(n);
                
                //Continue if we've allready visited this node
                if (hasVisited.contains(n2)) continue;
                
                //Add this node to the queue if it's not there yet
                if (!queue.contains(n2)) queue.offer(n2);

                //Calculate the cost to go to the new end node
                double alternative = costTo.get(n) + e.getWeight();

                //If no distance is recorded for this node yet, or the new distance
                //is shorter, update the list of distances to (distTo), and the
                //easiest path to the given node (cameFrom).
                if (!costTo.containsKey(n2) || alternative < costTo.get(n2)) {
                    costTo.put(n2, alternative);
                    queue.remove(n2);
                    queue.offer(n2);
                    cameFrom.put(n2, e);
                }
            }
        }
        //Return null if no path could be found.
        return null;
    }

    //Variables for aStar
    private List<E> aStar(N start, N end, Heuristics h) {
        //Initialize necessary data structures..
        HashSet<N> hasVisited = new HashSet<N>();
        final HashMap<N, E> cameFrom = new HashMap<N, E>();
        final HashMap<N, Double> gScore = new HashMap<N, Double>();
        final HashMap<N, Double> hScore = new HashMap<N, Double>();
        final HashMap<N, Double> fScore = new HashMap<N, Double>();

        //Add initial values
        gScore.put(start,0.0);
        fScore.put(start,hScore.get(start));

        //Setup the priority queue with comparison on the fScore
        //final PriorityQueue<N> queue = new PriorityQueue<N>(10000, new Comparator<N>() {
        //Replace java standard PriorityQueue with Sedgewick-Wayne version - alot faster
        //final MinPQ<N> queue = new MinPQ<N>(10000, new Comparator<N>() {
        final PairHeap<N> queue = new PairHeap<N>(10000, new Comparator<N>(){
            public int compare(N n1, N n2)
            {
                double v1;
                double v2;
                if (fScore.get(n1) == null) {
                    v1 = Double.POSITIVE_INFINITY;
                } else {
                    v1 = fScore.get(n1);
                }
                if (fScore.get(n2) == null) {
                    v2 = Double.POSITIVE_INFINITY;
                } else {
                    v2 = fScore.get(n2);
                }
                if (v1 > v2) {
                    return 1;
                } else if (v1 < v2) {
                    return -1;
                } else {
                    return 0;
                }
            }
        });

        queue.insert(start);

        while(!queue.isEmpty()){
            //Take the next element
            N n = queue.deleteMin();

            //Return the path if we're at the goal
            if(n==end) return getPathToTarget(end, cameFrom);

            hasVisited.add(n);

            for(E e : outGoingEdgesList(n)){
                N n2 = e.getOtherEndNode(n);

                //Continue if we've allready visited this node
                if (hasVisited.contains(n2)) continue;

                if(!gScore.containsKey(n2) || gScore.get(n) + e.getWeight() < gScore.get(n2)){
                    cameFrom.put(n2, e);
                    gScore.put(n2, gScore.get(n) + e.getWeight());
                    fScore.put(n2, gScore.get(n2)+h.h(n2,end));
                    if(queue.contains(n2)){
                        queue.updateDecreasedObject(n2);
                    }else{
                        queue.insert(n2);
                    }
                }
            }

        }
        return null;
    }

    /**
     * Get the path (of edges) to the target-node end
     * @param end The end node, for the path to arrive at
     * @return The path as an Iterable.
     */
    private List<E> getPathToTarget(N end, HashMap<N,E> edgeTo) {
        ArrayList<E> path = new ArrayList<E>();
        while (edgeTo.get(end) != null) {
            E e = edgeTo.get(end);
            if(e.getWeight() == 1.0E7) {
                return null;
            }
            path.add(e);
            end = e.getOtherEndNode(end);
        }
        ArrayList<E> finalPath = new ArrayList<E>();
        for(int i = path.size()-1; i>=0; i--){
            finalPath.add(path.get(i));
        }
        return finalPath;
    }
    
    /**
     * Plan the fastest route through a series of nodes
     * @param nodesToVisit The nodes to be visited on the road..
     * @return
     */
    public ArrayList<E> planTravel(List<N> nodesToVisit){
        if(1==1) {
            throw new UnsupportedOperationException("Not yet implemented");
        }
        return new ArrayList<E>();
    }

    public String toString() {
        return "Graph (" + this.hashCode() + ") has " + nodes.size() + " nodes and " + edges.size() + " edges.";
    }

    @Override
    public Graph<E, N> clone() {
        Graph<E, N> result = new Graph<E, N>(nodes, edges, reverse_edges, edgeCount);
        return result;
    }

    public Iterable<E> getEdges()
    {
        HashSet<E> res = new HashSet<E>();
        for(ArrayList<E> el : edges)
            for(E e : el)
                res.add(e);
        return res;
    }
}
